#pragma once

#include <Eigen/Eigen>
#include "Input_X.h"
#include "Output_X.h"
class Control_LQG
{
private:
    /* data */
    double PPS_offset;

    int cnt;
    int ctrl_interval;
    double LastTime;
    double u;
    int State;
    double dr;
    double delay;
    Eigen::Vector2d Estimate_X;
    Eigen::Matrix2d R;
    Eigen::Matrix2d P_old;
    Eigen::Matrix2d Qw;
    Eigen::Matrix2d F;
    Eigen::Matrix2d I;
    Eigen::Matrix2d H;
    Eigen::Matrix2d Fdr;
    Eigen::Vector2d B;
    Eigen::Vector2d G;
    Eigen::Vector2d L_sum;

public:
    Control_LQG(/* args */);
    ~Control_LQG();
    Output_X Input_meas(const Input_X &meas);
    double get_PPS_offset(){return this->PPS_offset;}
};


